% coreg_fid_cw
% Christopher J. Whalen
% University of Illinois
% Date Created ~ 01/01/04
% Date Modified - 09/13/05

% Purpose: Aligns elp coords with tai coords

% Aligning ELP to TAI Fiducails in BRIK_CTR space
% The convention will be tai and elp centers as 0,0,0  
% All positive rotations will be counterclockwise from view down the
% positive axis of rotation. All axies directions follow the BRIK coordinate system
% The positive axis of rotation for the last rotation will be from the
% origin to the Nasion of the TAI.

function [xyz] = coreg_fid_cw(xyz,tai)

% elp_matrix = x,y,z matrix of points with Na, LPA, RPA being 1st, 2nd, 3rd rows.
% tai_matrix = 3x3 matrix of x,y,z coords of the three fiducials Na, LPA, RPA.

fprintf('\nFiducial Coregistration Alignment In Progress...\n');
avg_map_err_init = mean(sqrt(sum(((tai.fid-xyz(1:3,:)).^2),2))); % Initial Map Error

num_dig_pts = size(xyz,1);
num_aberrant_pts = 0;

% fprintf('\ntai.origin = ');
% tai.origin
% fprintf('\nBefore Alignment tai.fid = ');
% tai.fid_b4 = tai.fid;
% tai.fid
% fprintf('\nBefore Alignment xyz = ');
% xyz_fid_b4 = xyz(1:3,:);
% xyz(1:3,:)
% fprintf('\nDiff Distance Matrix = ');
% diff_dist = [sqrt((tai.fid(1,1)-xyz(1,1))^2 + (tai.fid(1,2)-xyz(1,2))^2 + (tai.fid(1,3)-xyz(1,3))^2); 
%              sqrt((tai.fid(2,1)-xyz(2,1))^2 + (tai.fid(2,2)-xyz(2,2))^2 + (tai.fid(2,3)-xyz(2,3))^2);
%              sqrt((tai.fid(3,1)-xyz(3,1))^2 + (tai.fid(3,2)-xyz(3,2))^2 + (tai.fid(3,3)-xyz(3,3))^2)]

% X Rotation - Angular Origin on +Y axis. Counterclockwise positive. RHR with thumb in +X direction
%               Note - this is one Euler convention.
%              ~ Rotation about ears. Forehead down is positive.
dist_Na_elp = -1*xyz(1,2);   % distance from origin to Nasion. -1 b/c Na in neg direction from origin when using .BRIK coords
dist_Na_tai = sqrt( (tai.fid(1,1))^2 + (tai.fid(1,2))^2 + (tai.fid(1,3))^2 );
Na_z_tai = tai.fid(1,3);
x_rot = -1*asin(Na_z_tai/dist_Na_tai);  % -1 added so counterclockwise rot is positive number
fprintf('X rotation: %3.2f degrees \n',x_rot*360/(2*pi)); % -1 added to get correct direction
% Rotate All XYZ Points
Rx = [1 0 0;0 cos(x_rot) -sin(x_rot);0 sin(x_rot) cos(x_rot)];
for i=1:num_dig_pts
   xyz(i,:) = (Rx*(xyz(i,:)'))'; 
end
Rx_fin = Rx; % Storing to report final transformation


% Z Rotation - Angular Origin at +X axis. Counterclockwise positive. (this angle should be pretty small)
%              ~ Rotation about Neck. Rotation from right side of face to left is positive. 
z_rot = atan(tai.fid(1,1)/ (-1*tai.fid(1,2))); % Don't want atan2!
fprintf('Z rotation: %3.2f degrees \n',z_rot*360/(2*pi));
Rz = [cos(z_rot) -sin(z_rot) 0;sin(z_rot) cos(z_rot) 0;0 0 1];
for i=1:num_dig_pts
  xyz(i,:) = (Rz*(xyz(i,:))')';
end    
Rz_fin = Rz; % Storing to report final transformation


% Origin-Na Line Rotation - Pos axis toward Nasion. Angular Origin at ELP
%                           plane described by Na-(0,0,0)-LPA. Counterclockwise positive.
%                           ~ Rotation about Nose. Pos ic clockwise when looking at face and 'twisting' about nose

% Step 1: Find directional vector for line
L_unit = [tai.fid(1,1) tai.fid(1,2) tai.fid(1,3)]/(sqrt( (tai.fid(1,1))^2 + (tai.fid(1,2))^2 + (tai.fid(1,3))^2));
% line equation is L_line = t*L_unit

% Step 2: Find normal vectors of elp plane and tai plane
N_tai =  cross(tai.fid(2,:), tai.fid(1,:));
N_unit_tai = N_tai/(sqrt((N_tai(1,1)^2 +(N_tai(1,2))^2 + (N_tai(1,3))^2)));
N_elp = cross(xyz(2,:), xyz(1,:));
N_unit_elp = N_elp/(sqrt( (N_elp(1,1))^2 + (N_elp(1,2))^2 + (N_elp(1,3))^2));

% Step 3: Find the rotational angle between the two planes
theta = acos(dot(N_unit_tai,N_unit_elp));  % theta can only be pos by formula
if dot(cross(xyz(2,:), tai.fid(2,:)), tai.fid(1,:)) <= 0 
    theta = -1*theta;
elseif dot(cross(xyz(2,:), tai.fid(2,:)), tai.fid(1,:)) > 0
    theta = theta; % theta will be pos auto. must set to neg is appropriate
end
fprintf('Plane Angle: %3.2f degrees \n', theta*360/(2*pi));

% Obtaining Rotational Matrix
v = L_unit;
a = sqrt(v(1)^2 + v(2)^2);
Rx = [ v(1)/a v(2)/a 0; -v(2)/a v(1)/a 0; 0 0 1];
Ry = [v(3) 0 -a; 0 1 0; a 0 v(3)];
Rz = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
Rx_inv = inv(Rx);
Ry_inv = inv(Ry);
Rv = Rx_inv*Ry_inv*Rz*Ry*Rx; % Rotational Matrix that has to be applied to each point

% Step 4: Rotate all points about this line theta degrees
for i = 1:num_dig_pts
    xyz(i,:)= (Rv*(xyz(i,:)'))';
end    
Rv_fin = Rv; % Storing to report final transformation

Rtot = Rv_fin*Rz_fin*Rx_fin;

% Outliers - Check for a div by 0 outlier
% Check that by some odd chance one point had a near zero value making the
% numerical approx value be very large b/c div by ~0 so that a point became
% out of place
for i = 1:num_dig_pts
    while max(xyz(i,:)) > 250 % HACK JOB
       aberrant_pt = xyz(i,:);
       replacement_pt = xyz(round(rand(1)*i),:); %Choses a random replacement point
       xyz(i,:) = replacement_pt;    % Reassign aberrant pts. Replacing with a random point to keep matrix the same size. Not the best but not tragic
       num_aberrant_pts = num_aberrant_pts + 1;
   end
end
fprintf('Number Aberrant Points: %-3.0f',num_aberrant_pts);
if num_aberrant_pts > 0
  fprintf('Warning: Number Aberrant Points > 0.\n If coregistering ELP files this will be a problem. Contact Chris Whalen or other.\n\n');
end

avg_map_err_fin = mean(sqrt(sum(((tai.fid-xyz(1:3,:)).^2),2))); % Initial Map Error
fprintf('\nInitial Average Map Error: %3.1f mm  ', avg_map_err_init);
fprintf('\nFinal   Average Map Error: %3.1f mm\n', avg_map_err_fin );
if (avg_map_err_init < avg_map_err_fin)
    fprintf('\nError: Fiducial Coregistration Failed! (See CW)\n');
end


% fprintf('\nAfter Alignment tai.fid = ');
% tai.fid
% fprintf('\nAfter Alignment xyz = ');
% xyz(1:3,:)
% fprintf('\nDiff Distance Matrix = ');
% diff = tai.fid - xyz(1:3,:)
% 
% fprintf('\nDiff Distance Matrix = ');
% diff_dist = [sqrt((tai.fid(1,1)-xyz(1,1))^2 + (tai.fid(1,2)-xyz(1,2))^2 + (tai.fid(1,3)-xyz(1,3))^2); 
%              sqrt((tai.fid(2,1)-xyz(2,1))^2 + (tai.fid(2,2)-xyz(2,2))^2 + (tai.fid(2,3)-xyz(2,3))^2);
%              sqrt((tai.fid(3,1)-xyz(3,1))^2 + (tai.fid(3,2)-xyz(3,2))^2 + (tai.fid(3,3)-xyz(3,3))^2)]
% 

% xyz = round(xyz); % round to fit in voxel space/coords







